# 循迹初代 - 进阶 - 过桥 - 倒车 - By: LUCKBACK - Sun May 12 2024
# 缺点：会受到点的干扰，偏差过大失效

import sensor, image, time, json # type: ignore
from machine import UART # type: ignore
from SerialData import SerialData # type: ignore
from image import SEARCH_EX, SEARCH_DS # type: ignore
import settings,algorithm

uart1 = UART(1,115200)
uart2 = UART(2,115200)

uart1data = SerialData(uart1)
uart2data = SerialData(uart2)

# 加载模板
num_quantity = 9   #0~8 模板个数
num_model=[]    #模板列表
#加载模板
#模板应该是一个小的灰度图像，如32×32.
#template = image.Image("/1.pgm")#模板库 要求图片格式为pgm 需要下载到sd卡中
num_model.append(image.Image("/0.pgm"))
num_model.append(image.Image("/1.pgm"))
num_model.append(image.Image("/2.pgm"))
num_model.append(image.Image("/3.pgm"))
num_model.append(image.Image("/4.pgm"))
num_model.append(image.Image("/5.pgm"))
num_model.append(image.Image("/6.pgm"))
num_model.append(image.Image("/7.pgm"))
num_model.append(image.Image("/8.pgm"))

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.set_auto_whitebal(True)
sensor.skip_frames(time = 2000)

clock = time.clock()

img_gray = sensor.alloc_extra_fb(160,120,sensor.GRAYSCALE)  # 为灰度设置一个画布
img_to_matching = sensor.alloc_extra_fb(35,45,sensor.GRAYSCALE) #声明灰度画布用于匹配模板

position = 80    # 最终输出的位置
position_red = 80    # 过桥模式通过红线算出的位置
position_green = 80  # 过桥模式通过绿线算出的位置
red_flag = 0    # 判断是否有红线
pointFlag = 0   # 是否找到点

error = 0   # 0表示有线,1表示没找到线

trace_sta = 0
trace_x = 0
trace_y = 0

scale = 1 #缩放比例
last_number = []
number = []
count = 0

knife_sta = 0

save_count = 0
save_number = 0

auto_state = 0
Distance = 1

# 对摄像头的初始化
def camera_init():
    global clock
    global img
    global img_gray

    clock.tick()
    img = sensor.snapshot()
    # print(clock.fps())

    img_gray.draw_image(img,0,0)    # 获取灰度图

    histo = img_gray.get_histogram() # 获取直方图
    Threshold = histo.get_threshold()   # 获取阈值
    hisv = Threshold.value()            # 获取阈值
    # print(hisv)
    return hisv

# 通过色差查看是否有白线
def judge_whiteLine():
    blobs_1 = img_gray.find_blobs([(hisv,255)],invert = False,\
                        roi = (0,100,160,5),x_stride = 5,y_stride = 5,\
                        area_thresholds = 50,pixels_thresholds = 50,merge = True)
    blobs_2 = img_gray.find_blobs([(hisv,255)],invert = True,\
                        roi = (0,100,160,5),x_stride = 5,y_stride = 5,\
                        area_thresholds = 50,pixels_thresholds = 50,merge = True)
    if(len(blobs_1) and len(blobs_2)):
        blob_1 = max(blobs_1,key = lambda b: b.density())
        blob_2 = max(blobs_2,key = lambda b: b.density())
        img.draw_rectangle(blob_1.rect(),color = (0,255,0))
        img.draw_rectangle(blob_2.rect(),color = (0,255,0))
        chromatism = algorithm.get_chromatism(img,blob_1,blob_2)
        print(chromatism)
        if(chromatism > settings.CHROMATISM_THRESHOLD):
            return True
        else:
            return False
    else:
        return False


def common_findLine():
    # 查找三次图块，取平均值
    global pointFlag    # 是否找到点
    global position # 最终输出的位置
    global error    # 是否有线
    x = [] # 3个色块中心的x值
    y = [] # 3个色块中心的y值
    line_weight = []   # 线的宽度
    findLine = 0

    # 通过色差查看是否有白线
    if(judge_whiteLine() == False):
        print("no line")
        error = 1
        position = 80
        return 0
    else:
        error = 0

    for i in range(9,12):
        # 这里用的是img_gray查找色块
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,10*i,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        # 当色块只有一个的时候存下来，两个以上判断最接近中间的那个点，否则就存入中间值
        if (len(blobs) >= 1):
            blob = max(blobs,key = lambda b: -abs(80 - b.cx()))
            weight = blob.w()
            line_weight.append(weight)
            img.draw_rectangle(blob.rect(),color = (255,0,0))
            x.append(blob.cx())
            y.append(blob.cy())
            if(weight > settings.LINE_MAX_WEIGHT):
                pointFlag = 1

    # 检查宽度，若宽度相差过大，则剔除最大宽度的数据
    print(line_weight)
#    print(x)
    if (pointFlag == 0):    # 没找到点时滤波，排除干扰
        if (len(line_weight) > 1):
            max_weight = max(line_weight)
            different_wei = max_weight - (sum(line_weight) - max_weight) / (len(line_weight)-1)
            if (different_wei > settings.LINE_DIFFERENCE):
                print("in")
                position = (sum(x) - x[line_weight.index(max_weight)]) / (len(x)-1)
            else:
                position = sum(x) / len(x)  # 相加取平均值
        elif (len(line_weight) == 1):
            position = sum(x) / len(x)  # 相加取平均值
    elif (pointFlag == 1):  # 找到点时进入特殊情况，找最上面的点
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,settings.WANRING_Y,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        if (len(blobs) > 0):
            blob = max(blobs,key = lambda b: -abs((sum(x) / len(x)) - b.cx()))
            img.draw_rectangle(blob.rect(),color = (255,0,0))
            position = blob.cx()

    position = algorithm.position_limit(position)
    return 0

# 过桥模式查找中间区域(波动较大)(暂时舍弃查找中间区域，因为高度会影响识别)
def brige_findMiddle():
    global position_green  # 过桥模式通过绿线算出的位置
    x = [] # 3个色块中心的x值
    y = [] # 3个色块中心的y值
    for i in range(5,8):
        # 这里用的是img查找色块     # 中间区域是暗绿色
        blobs = img.find_blobs([settings.GREEN_THRESHOLD],invert = False,\
                                roi = (30,10*i,100,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        # 当色块只有一个的时候才存下来，否则就存入中间值
        if (len(blobs) >= 1):
            blob = max(blobs,key = lambda b: -abs(80 - b.cx()))
            img.draw_rectangle(blob.rect())
            x.append(blob.cx())
            y.append(blob.cy())

    if (len(x) != 0):
        position_green = sum(x) / len(x) # 取平均值
    else:
        position_green = 80
    return 0

# 过桥模式查找红线,search_y为输入BRIGE_Y或SEESAW_Y
# find_one为输入是否通过点查找红线，True为是，False为否
def brige_findRedLine(threshold,th_invert,search_y,find_one):
    global position    # 最终输出的位置
    global position_red    # 过桥模式通过红线算出的位置
    global red_flag    # 判断是否有红线
    size = 0    # 过桥模式检测到的红色块的数量
    red_x = []  # 存储只有一条红线的情况时的横坐标
    red_y = []  # 存储只有一条红线的情况时的纵坐标
    for i in range(search_y-3,search_y):
        blobs = img.find_blobs([threshold],roi = (0,i*20,160,10),invert = th_invert,\
                                x_stride = 5,y_stride = 5,area_thresholds = 200,\
                                pixels_thresholds = 200,merage = True)
        if(len(blobs) == 2):
            red_flag = 1
            for blob in blobs:
                img.draw_rectangle(blob.rect())
                position_red += blob.cx()
            size += 1
        elif(len(blobs) == 1 and find_one == True):
            img.draw_rectangle(blobs[0].rect())
            red_x.append(blobs[0].cx())
            red_y.append(blobs[0].cy())

    if(size != 0):
        position_red /= (2*size)
#        print(position_red)
        size = 0
    elif(len(red_x)>1 and find_one == True):
        [c,d] = algorithm.least_squares(red_x,red_y)
        if(c < 0):
            position_red = (sum(red_x) + 160*3)/(2*len(red_x))
        elif(c > 0):
            position_red = sum(red_x)/(2*len(red_x))
        else:
            position_red = 80
    else:
        position_red = 80
        size = 0
    return 0

def back_findLine():
    global position
    Reversing_x = [80,80,80]   # 倒车模式的x坐标
    Reversing_y = [80,100,120] # 倒车模式的y坐标
    # 这里用的是img_gray查找色块  90到100,100到110,110到120
    for i in range(0,3):
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,90+i*10,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        # 当色块只有一个的时候存下来，两个以上判断最接近中间的那个点，否则就存入中间值
        if (len(blobs) == 1):#一个色块
            img.draw_rectangle(blobs[0].rect())
            Reversing_x[i] = blobs[0].cx()#远点坐标
            Reversing_y[i] = blobs[0].cy()
        elif (len(blobs) >= 2):   #多个色块
            blob = max(blobs,key = lambda b: -abs(80 - b.cx()))#两个以上判断最接近中间的那个点
            img.draw_rectangle(blob.rect())
            Reversing_x[i] = blob.cx()#远点坐标
            Reversing_y[i] = blob.cy()
#            else:
#                Reversing_x[i] = 80#远点坐标
#                Reversing_y[i] = 120 - 75 + 20 * i
    # 通过最小二乘法，算出斜率倒数c和截距d
    if (len(Reversing_x) != 0):
        [c,d] = algorithm.least_squares(Reversing_x,Reversing_y)
        position = -120 * c + d
        img.draw_line((int(0*c+d),0,int(120*c+d),120),color = (255,0,0))
    #限制定义域
    position = algorithm.limit(position,0,160)
#        for i in range(0,3):
#            print(Reversing_x[i])
#            print(Reversing_y[i])
    return 0

def trace_r_findLine():
    global position # 最终输出的位置
    position = 0
    for i in range(6,9):
        # 这里用的是img_gray查找色块
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,10*i,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        # 当色块只有一个的时候存下来，两个以上判断最接近右边的那个点，否则就存入中间值
        if blobs:
            blob = max(blobs,key = lambda b: b.cx())
            img.draw_rectangle(blob.rect())
            position += blob.cx()
        else:
            position += 80
    position /= 3
    print(position)
    return 0

def trace_l_findLine():
    global position # 最终输出的位置
    position = 0
    for i in range(6,9):
        # 这里用的是img_gray查找色块
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,10*i,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = True)
        # 当色块只有一个的时候存下来，两个以上判断最接近左边的那个点，否则就存入中间值
        if blobs:
            blob = min(blobs,key = lambda b: b.cx())
            img.draw_rectangle(blob.rect())
            position += blob.cx()
        else:
            position += 80
    position /= 3
    print(position)
    return 0

def identify_number():
    global number
    global last_number
    global count
    global scale
    threshold = (0,hisv-10)
    blobs = img_gray.find_blobs([threshold])
    blobs.sort(key = lambda b:b.cx())
    for blob in blobs:
        if blob.pixels()>50 and 100>blob.h()>10 and blob.w()>3:
            scale = 40/blob.h()
            img_to_matching.draw_image(img,0,0,roi=(blob.x()-1,blob.y()-1,blob.w()+2,blob.h()+2),x_scale=scale,y_scale=scale)
            for n in range(0,num_quantity):
                r = img_to_matching.find_template(num_model[n], 0.70, step=2, search=SEARCH_EX)
                if r:
                    number.append(n)
                    img.draw_rectangle(blob.x()-2,blob.y()-2,blob.w()+4,blob.h()+4,color=(42*n,255,0))
                    img.draw_string(blob[0],blob[1]-13,str(n),scale=1,color=(42*n,255,0))
#    print(clock.fps())
    if(len(number) == 1):
#        print(number)
        if len(list(set(last_number) & set(number))) == 1:
            count+=1
            if count > 4 and len(number) == 1:
                count = 0
                print(number)
                uart2data.toSendData(number = number[0])
        else:
            count = 0
        last_number = number
    number.clear()
    return 0

def knife_findRedLine(threshold,th_invert):
    global position
    global position_red    # 过桥模式通过红线算出的位置
    global pointFlag    # 是否找到点
    global knife_sta
    if knife_sta == 0:
        #彩度找红块
        blobs = img.find_blobs([threshold],roi=(0,80,160,10),\
        x_stride=5,y_stride=5,invert=th_invert,pixels_threshold =50,merge=True,margin=1)
        img.draw_rectangle((0,80,160,10),color=(0,255,0),thickness = 1)
        #上方找到红线
        if blobs:
            knife_sta = 1
        else:
            #先普通循迹
            common_findLine()
    if knife_sta == 1:
        #刀山循迹
        blobs = img.find_blobs([threshold],roi=(0,60,160,20),\
        x_stride=5,y_stride=5,invert=th_invert,pixels_threshold =50,merge=True,margin=1)
        if blobs and len(blobs)==2:
            for blob in blobs:
                position_red += blob.cx()
                img.draw_rectangle(blob.rect(),color=(0,255,0),thickness = 1)
            position_red /= 2
            img.draw_line(int(position_red),80,int(position_red),120,color=(0,255,255))
        else:
            knife_sta = 2
        position = position_red
    if knife_sta == 2:
        #刀山循迹
        blobs = img.find_blobs([threshold],roi=(0,110,160,10),\
        x_stride=5,y_stride=5,invert=th_invert,pixels_threshold =50,merge=True,margin=1)
        if blobs and len(blobs)==2:
            for blob in blobs:
                position_red += blob.cx()
                img.draw_rectangle(blob.rect(),color=(0,255,0),thickness = 1)
            position_red /= 2
        else:
            position = 80
            pointFlag = 1
            knife_sta = 0
            return 1
        position = position_red
    return 0

def platform_correct():
    global position # 最终输出的位置
    blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                            roi = (0,0,160,10),x_stride = 5,y_stride = 5,\
                            area_thresholds = 50,pixels_thresholds = 50,merge = True)
    if (len(blobs) > 0):
        blob = max(blobs,key = lambda b: -abs(80 - b.cx()))
        img.draw_rectangle(blob.rect(),color = (255,0,0))
        position = blob.cx()
    return 0

def hill_correct(threshold,th_invert):
    global position # 最终输出的位置
    global pointFlag
    # 找红色块
    blobs = img.find_blobs([threshold],invert = th_invert,\
                            roi = (40,50,80,70),x_stride = 5,y_stride = 5,\
                            area_thresholds = 200,pixels_thresholds = 200,merge = True)
    if blobs and len(blobs) >= 1:
        blob = max(blobs,key = lambda b: -abs(80 - b.cx()))
        y = blob.y()
        img.draw_rectangle(blob.rect(),color=(255,255,0),thickness = 1)
        #print(hisv)
        # 找红色块前的白色块
        blobs = img_gray.find_blobs([(hisv+75,255)],invert = False,\
                                    roi = (20,y-10,120,10),x_stride = 5,y_stride = 5,\
                                    area_thresholds = 200,pixels_thresholds = 200,merge = True)
        #print(len(blobs))
        #for blob in blobs:
        #    img.draw_rectangle(blob.rect(),color=(255,0,0),thickness = 1)
        if blobs and len(blobs) >= 1:
            blob = max(blobs,key = lambda b: b.density())
            img.draw_rectangle(blob.rect(),color=(0,255,0),thickness = 1)
            position = blob.cx()
        if y > 100:
            pointFlag = 1
        else:
            pointFlag = 0
        print(pointFlag)
    return 0

def find_point1(find_node_y,line_distance_weight):
    line_weight = []   # 线的宽度
    # 这里用的是img_gray查找色块
    blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                            roi = (10,find_node_y,140,7),x_stride = 5,y_stride = 5,\
                            area_thresholds = 50,pixels_thresholds = 50,merge = True)
    # 当色块只有一个的时候存下来，两个以上判断最接近中间的那个点，否则就存入中间值
    if (len(blobs) >= 1):
        blob = max(blobs,key = lambda b: -abs((position) - b.cx()))
        weight = blob.w()
        line_weight.append(weight)
        img.draw_rectangle(blob.rect(),color = (255,0,0))
        if(weight > line_distance_weight):
            return 1
    return 0

# 通过两个直线的两个不同的点来查找交点,
# 参数y_search为扫描的y值,在0到MIDDLE_FIND_NODE_Y-10之间取
# 放回交点的y值
def find_point2(y_search):
    x = [0,0,0,0]   # 中心x值
    y = [0,0,0,0]   # 中心y值
    w = [0,0,0,0]   # 色块宽度
    find = 0

    for i in range(0,2):
        blobs = img_gray.find_blobs([(hisv,255)],invert = False,\
                                roi = (0,y_search+i*10,160,10),x_stride = 5,y_stride = 5,\
                                area_thresholds = 50,pixels_thresholds = 50,merge = False)
        if(len(blobs) >= 2):
            find+=1
            x[i] = blobs[0].cx()
            y[i] = blobs[0].cy()
            w[i] = blobs[0].w()
            x[i+2] = blobs[1].cx()
            y[i+2] = blobs[1].cy()
            w[i+2] = blobs[1].w()
            for blob in blobs:
                img.draw_rectangle(blob.rect(),color = (255,0,0))

    if(find == 2):
        if(w[0]-w[1] < 10 and w[2]-w[3] < 10):  # 防止到边缘测量不准
            [x0,y0] = algorithm.Get_Point(x,y)
            img.draw_cross(int(x0),int(y0),color = (255,0,0))
            return y0
    return 0

# 中距离找点也会进行近距离找点，远距离找点也会进行中距离和近距离找点
def findPoint():
    global pointFlag
    line_weight = []   # 线的宽度
    # 近距离找点
    if(uart2data.toFindPoint() == 1 or pointFlag != 0):
        return pointFlag

    cross_y = find_point2(settings.POINT_SEARCH_Y)
    if(uart2data.toFindPoint() == 2 and cross_y > settings.MIDDLE_FIND_NODE_Y):
        print(f"in{cross_y}")
        pointFlag = 1
        return pointFlag
    if(uart2data.toFindPoint() == 3 and cross_y > settings.REMOTE_FIND_NODE_Y):
        pointFlag = 1
        return pointFlag

    # 如果cross_y不为0,说明老方法会提前找到点,于是弃用老方法
    if(0<cross_y<120):
        return pointFlag

    # 中距离找点
    pointFlag = find_point1(settings.MIDDLE_FIND_NODE_Y,settings.LINE_MIDDLE_DISTANCE_WEIGHT)
    if(uart2data.toFindPoint() == 2 or pointFlag != 0):
        print(line_weight)
        return pointFlag

    # 远距离找点
    pointFlag = find_point1(settings.REMOTE_FIND_NODE_Y,settings.LINE_REMOTE_DISTANCE_WEIGHT)
    if(uart2data.toFindPoint() == 3 or pointFlag != 0):
        print(line_weight)
        return pointFlag

# 识别二维码
def identify_qrcode():
    code = img.find_qrcodes()
    if(len(code) == 1):
        img.draw_rectangle(code[0].rect(), color = (255, 0, 0))
        uart2data.toSendData(qrcode = code[0].payload())

# 如果直接找到最近的观测点,就不会再找了,返回的是距离
def auto_distance_point():
    global auto_state
    for i in range(settings.AUTO_SIZE-1,auto_state-1,-1):
        if(find_point1(settings.AUTO_Y[i],settings.AUTO_POINT_WEIGHT[i])):
            auto_state = i+1
            return settings.AUTO_DISTANCE[i]
    return 1

# 根据点来获取距离
def get_distance_point():
    global Distance
    global auto_state
    common_findLine()
    Distance = auto_distance_point()
    if(auto_state >= settings.AUTO_MODE):
        auto_state=0
        uart2data.changeFindLineMode(1)
    return 0

def find_roadblock(threshold,find_roadblock):
    blobs = img.find_blobs([threshold],invert = False,\
                        roi = (10,find_roadblock,140,7),x_stride = 5,y_stride = 5,\
                        area_thresholds = 200,pixels_thresholds = 200,merge = True)
    if(len(blobs) >= 1):
        blob = max(blobs,key = lambda b: b.w())
        img.draw_rectangle(blob.rect())
        if(blob.w() > 30):
            return 1
    return 0

# 自动查找距离(障碍模式)
def auto_distance_roadblock():
    global auto_state
    for i in range(settings.AUTO_SIZE-1,auto_state-1,-1):
        if(find_roadblock(settings.BLACK_THRESHOLD,settings.AUTO_Y[i])):
            auto_state = i+1
            return settings.AUTO_DISTANCE[i]
    return 1

# 获取距离模式(障碍模式)
def get_distance_roadblock():
    global Distance
    global auto_state
    common_findLine()
    Distance = auto_distance_roadblock()
    if(auto_state >= settings.AUTO_MODE):
        auto_state=0
        uart2data.changeFindLineMode(1)
    return 0

def savePicture():
    global save_count
    global save_number
    global img
    save_count += 1
    if(save_count >= 20):
        save_count = 0
        save_number += 1
        img.save(f"/Pic/{save_number}_{position}.jpg")

while(True):
    uart2data.getUartData()

    while (uart2data.getUartData() == 0):
        result = 0
        mode = uart2data.getFindLineMode()
        # 识别数字
        if (uart2data.toIdentifyNumber() == 1):
            hisv = camera_init()
            result = identify_number()
            continue
        # 识别二维码
        if (uart2data.toSearchqrcode() == 1):
            hisv = camera_init()
            result = identify_qrcode()
            continue
        # 正常模式
        if (mode == 1):
            hisv = camera_init()
            result = common_findLine()
        # 过桥模式
        if (mode == 2):
            hisv = camera_init()
            result = brige_findRedLine(settings.BRIDGE_THRESHOLD,settings.BRIDGE_THRESHOLD_INVERT,settings.BRIGE_Y,True)
            position_green = 80 # 暂时舍弃查找中间区域，因为高度会影响识别
            position = position_red  # 要么是红色加绿色
            position = algorithm.position_limit(position)
        # 倒车模式
        if (mode == 3):
            hisv = camera_init()
            result = back_findLine()
            position = algorithm.position_limit(position)
        # 跷跷板模式
        if (mode == 4):
            hisv = camera_init()
            result = brige_findRedLine(settings.SEESAW_THRESHOLD,settings.SEESAW_THRESHOLD_INVERT,settings.SEESAW_Y,False)
            position = position_red
            position = algorithm.position_limit(position)
        # 右抓线模式
        if (mode == 5):
            hisv = camera_init()
            result = trace_r_findLine()
            position = algorithm.position_limit(position)
        # 左抓线模式
        if (mode == 6):
            hisv = camera_init()
            result = trace_l_findLine()
            position = algorithm.position_limit(position)
        # 平台矫正模式
        if (mode == 7):
            hisv = camera_init()
            result = hill_correct(settings.RED_THRESHOLD,settings.RED_THRESHOLD_INVERT)
            position = algorithm.position_limit(position)
        # 山丘矫正模式
        if (mode == 8):
            hisv = camera_init()
            result = hill_correct(settings.RED_THRESHOLD,settings.RED_THRESHOLD_INVERT)
            position = algorithm.position_limit(position)
        # 刀山模式
        if (mode == 9):
            hisv = camera_init()
            result = knife_findRedLine(settings.RED_THRESHOLD,settings.RED_THRESHOLD_INVERT)
            position = algorithm.position_limit(position)
        # 获取距离模式(点模式)
        if (mode == 10):
            hisv = camera_init()
            result = get_distance_point()
            position = algorithm.position_limit(position)
        if (mode == 11):
            hisv = camera_init()
            result = get_distance_roadblock()
            position = algorithm.position_limit(position)

        print(position)

        # 查看是否要发送点
        if (mode == 10 or mode == 11):
            uart2data.toSendData(line = int(position),Error = error,distance = Distance)
        elif (uart2data.toFindPoint() != 0 or pointFlag != 0):
            uart2data.toSendData(line = int(position),Error = error,node = findPoint())
        else:
            uart2data.toSendData(line = int(position),Error = error)
        position = 80
        position_red = 0
        position_green = 0
        pointFlag = 0
        error = 0

        if (result == 1):
            while (uart2data.getUartData() == 1):
                pass
            break

    auto_state=0


